/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.frames;

import ore.Transform;
import ore.errors.OrekitException;
import ore.time.AbsoluteDate;

import org.apache.commons.math.geometry.Rotation;
import org.apache.commons.math.geometry.Vector3D;

/** 
 * True Equator, Mean Equinox of Date Frame, sometimes called True of
 * Date (ToD) frame.  This frame handles nutation effects according to
 * the IAU-80 theory.  Its parent frame is the {@link MEMEFrame}.
 * 
 * This implementation includes a caching/interpolation feature to
 * tremendously improve efficiency. The IAU-80 theory involves lots of
 * terms (106 components for Dpsi and Deps). Recomputing all these
 * components for each point is really slow. The shortest period for
 * these components is about 5.5 days (one fifth of the moon
 * revolution period), hence the pole motion is smooth at the day or
 * week scale. This implies that these motions can be computed
 * accurately using a few reference points per day or week and
 * interpolated between these points. This implementation uses 12
 * points separated by 1/2 day (43200 seconds) each, the resulting
 * maximal interpolation error on the frame is about
 * 1.3&times;10<sup>-10</sup> arcseconds.
 * 
 * @author Pascal Parraud
 */
public class TEMEFrame
    extends NevilleFrame
{
    /*
     * Coefficients for the Mean Obliquity of the Ecliptic.
     */
    private static final double MOE_0 = 84381.448    * RADIANS_PER_ARC_SECOND;
    private static final double MOE_1 =   -46.8150   * RADIANS_PER_ARC_SECOND;
    private static final double MOE_2 =    -0.00059  * RADIANS_PER_ARC_SECOND;
    private static final double MOE_3 =     0.001813 * RADIANS_PER_ARC_SECOND;
    /*
     * LuniSolar nutation elements
     */
    /*
     * Coefficients for l (Mean Anomaly of the Moon).
     */
    private static final double F10  = Math.toRadians(134.96298139);
    private static final double F110 =    715922.633 * RADIANS_PER_ARC_SECOND;
    private static final double F111 =      1325.0;
    private static final double F12  =        31.310 * RADIANS_PER_ARC_SECOND;
    private static final double F13  =         0.064 * RADIANS_PER_ARC_SECOND;
    /*
     * Coefficients for l' (Mean Anomaly of the Sun).
     */
    private static final double F20  = Math.toRadians(357.52772333);
    private static final double F210 =   1292581.224 * RADIANS_PER_ARC_SECOND;
    private static final double F211 =        99.0;
    private static final double F22  =        -0.577 * RADIANS_PER_ARC_SECOND;
    private static final double F23  =        -0.012 * RADIANS_PER_ARC_SECOND;
    /*
     * Coefficients for F = L (Mean Longitude of the Moon) - Omega.
     */
    private static final double F30  = Math.toRadians(93.27191028);
    private static final double F310 =    295263.137 * RADIANS_PER_ARC_SECOND;
    private static final double F311 =      1342.0;
    private static final double F32  =       -13.257 * RADIANS_PER_ARC_SECOND;
    private static final double F33  =         0.011 * RADIANS_PER_ARC_SECOND;
    /*
     * Coefficients for D (Mean Elongation of the Moon from the Sun).
     */
    private static final double F40  = Math.toRadians(297.85036306);
    private static final double F410 =   1105601.328 * RADIANS_PER_ARC_SECOND;
    private static final double F411 =      1236.0;
    private static final double F42  =        -6.891 * RADIANS_PER_ARC_SECOND;
    private static final double F43  =         0.019 * RADIANS_PER_ARC_SECOND;
    /*
     * Coefficients for Omega (Mean Longitude of the Ascending Node of
     * the Moon).
     */
    private static final double F50  = Math.toRadians(125.04452222);
    private static final double F510 =   -482890.539 * RADIANS_PER_ARC_SECOND;
    private static final double F511 =        -5.0;
    private static final double F52  =         7.455 * RADIANS_PER_ARC_SECOND;
    private static final double F53  =         0.008 * RADIANS_PER_ARC_SECOND;


    /** 
     * Coefficients of l, mean anomaly of the Moon.
     */
    private static final int[] CL = {
        +0,  0, -2,  2, -2,  1,  0,  2,  0,  0,
        +0,  0,  0,  2,  0,  0,  0,  0,  0, -2,
        +0,  2,  0,  1,  2,  0,  0,  0, -1,  0,
        +0,  1,  0,  1,  1, -1,  0,  1, -1, -1,
        +1,  0,  2,  1,  2,  0, -1, -1,  1, -1,
        +1,  0,  0,  1,  1,  2,  0,  0,  1,  0,
        +1,  2,  0,  1,  0,  1,  1,  1, -1, -2,
        +3,  0,  1, -1,  2,  1,  3,  0, -1,  1,
        -2, -1,  2,  1,  1, -2, -1,  1,  2,  2,
        +1,  0,  3,  1,  0, -1,  0,  0,  0,  1,
        +0,  1,  1,  2,  0,  0
    };
    /** 
     * Coefficients of l', mean anomaly of the Sun.
     */
    private static final int[] CLP = {
        +0,  0,  0,  0,  0, -1, -2,  0,  0,  1,
        +1, -1,  0,  0,  0,  2,  1,  2, -1,  0,
        -1,  0,  1,  0,  1,  0,  1,  1,  0,  1,
        +0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
        +0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
        +1,  1, -1,  0,  0,  0,  0,  0,  0,  0,
        -1,  0,  1,  0,  0,  1,  0, -1, -1,  0,
        +0, -1,  1,  0,  0,  0,  0,  0,  0,  0,
        +0,  0,  0,  1,  0,  0,  0, -1,  0,  0,
        +0,  0,  0,  0,  1, -1,  0,  0,  1,  0,
        -1,  1,  0,  0,  0,  1
    };
    /** 
     * Coefficients of F = L - &Omega;, where L is the mean longitude
     * of the Moon.
     */
    private static final int[] CF = {
        0,  0,  2, -2,  2,  0,  2, -2,  2,  0,
        2,  2,  2,  0,  2,  0,  0,  2,  0,  0,
        2,  0,  2,  0,  0, -2, -2,  0,  0,  2,
        2,  0,  2,  2,  0,  2,  0,  0,  0,  2,
        2,  2,  0,  2,  2,  2,  2,  0,  0,  2,
        0,  2,  2,  2,  0,  2,  0,  2,  2,  0,
        0,  2,  0, -2,  0,  0,  2,  2,  2,  0,
        2,  2,  2,  2,  0,  0,  0,  2,  0,  0,
        2,  2,  0,  2,  2,  2,  4,  0,  2,  2,
        0,  4,  2,  2,  2,  0, -2,  2,  0, -2,
        2,  0, -2,  0,  2,  0
    };
    /** 
     * Coefficients of D, mean elongation of the Moon from the Sun.
     */
    private static final int[] CD = {
        +0,  0,  0,  0,  0, -1, -2,  0, -2,  0,
        -2, -2, -2, -2, -2,  0,  0, -2,  0,  2,
        -2, -2, -2, -1, -2,  2,  2,  0,  1, -2,
        +0,  0,  0,  0, -2,  0,  2,  0,  0,  2,
        +0,  2,  0, -2,  0,  0,  0,  2, -2,  2,
        -2,  0,  0,  2,  2, -2,  2,  2, -2, -2,
        +0,  0, -2,  0,  1,  0,  0,  0,  2,  0,
        +0,  2,  0, -2,  0,  0,  0,  1,  0, -4,
        +2,  4, -4, -2,  2,  4,  0, -2, -2,  2,
        +2, -2, -2, -2,  0,  2,  0, -1,  2, -2,
        +0, -2,  2,  2,  4,  1
    };
    /** 
     * Coefficients of &Omega;, mean longitude of the ascending node of the Moon.
     */
    private static final int[] COM = {
        1, 2, 1, 0, 2, 0, 1, 1, 2, 0,
        2, 2, 1, 0, 0, 0, 1, 2, 1, 1,
        1, 1, 1, 0, 0, 1, 0, 2, 1, 0,
        2, 0, 1, 2, 0, 2, 0, 1, 1, 2,
        1, 2, 0, 2, 2, 0, 1, 1, 1, 1,
        0, 2, 2, 2, 0, 2, 1, 1, 1, 1,
        0, 1, 0, 0, 0, 0, 0, 2, 2, 1,
        2, 2, 2, 1, 1, 2, 0, 2, 2, 0,
        2, 2, 0, 2, 1, 2, 2, 0, 1, 2,
        1, 2, 2, 0, 1, 1, 1, 2, 0, 0,
        1, 1, 0, 0, 2, 0
    };
    /** 
     * Coefficients for nutation in longitude, const part, in 0.1milliarcsec.
     */
    private static final double[] SL = {
        -171996.0, 2062.0, 46.0,   11.0,  -3.0,  -3.0,  -2.0,   1.0,  -13187.0, 1426.0,
        -517.0,    217.0,  129.0,  48.0,  -22.0,  17.0, -15.0, -16.0, -12.0,   -6.0,
        -5.0,      4.0,    4.0,   -4.0,    1.0,   1.0,  -1.0,   1.0,   1.0,    -1.0,
        -2274.0,   712.0, -386.0, -301.0, -158.0, 123.0, 63.0,  63.0, -58.0,   -59.0,
        -51.0,    -38.0,   29.0,   29.0,  -31.0,  26.0,  21.0,  16.0, -13.0,   -10.0,
        -7.0,      7.0,   -7.0,   -8.0,    6.0,   6.0,  -6.0,  -7.0,   6.0,    -5.0,
        +5.0,     -5.0,   -4.0,    4.0,   -4.0,  -3.0,   3.0,  -3.0,  -3.0,    -2.0,
        -3.0,     -3.0,    2.0,   -2.0,    2.0,  -2.0,   2.0,   2.0,   1.0,    -1.0,
        +1.0,     -2.0,   -1.0,    1.0,   -1.0,  -1.0,   1.0,   1.0,   1.0,    -1.0,
        -1.0,      1.0,    1.0,   -1.0,    1.0,   1.0,  -1.0,  -1.0,  -1.0,    -1.0,
        -1.0,     -1.0,   -1.0,    1.0,   -1.0,   1.0
    };
    /** 
     * Coefficients for nutation in longitude, t part, in 0.1milliarcsec.
     */
    private static final double[] SLT = {
        -174.2,  0.2,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0, -1.6, -3.4,
        +1.2,   -0.5,  0.1, 0.0, 0.0, -0.1, 0.0, 0.1,  0.0,  0.0,
        +0.0,    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0,  0.0,
        -0.2,    0.1, -0.4, 0.0, 0.0,  0.0, 0.0, 0.1, -0.1,  0.0,
        +0.0,    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0,  0.0,
        +0.0,    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0,  0.0,
        +0.0,    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0,  0.0,
        +0.0,    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0,  0.0,
        +0.0,    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0,  0.0,
        +0.0,    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0,  0.0,
        +0.0,    0.0,  0.0, 0.0, 0.0,  0.0
    };
    /** 
     * Coefficients for nutation in obliquity, const part, in 0.1milliarcsec.
     */
    private static final double[] CO = {
        +92025.0, -895.0, -24.0,  0.0,    1.0,   0.0,   1.0,   0.0,   5736.0, 54.0,
        +224.0,   -95.0,  -70.0,  1.0,    0.0,   0.0,   9.0,   7.0,   6.0,    3.0,
        +3.0,     -2.0,   -2.0,   0.0,    0.0,   0.0,   0.0,   0.0,   0.0,    0.0,
        +977.0,   -7.0,    200.0, 129.0, -1.0,  -53.0, -2.0,  -33.0,  32.0,   26.0,
        +27.0,     16.0,  -1.0,  -12.0,   13.0, -1.0,  -10.0, -8.0,   7.0,    5.0,
        +0.0,     -3.0,    3.0,   3.0,    0.0,  -3.0,   3.0,   3.0,  -3.0,    3.0,
        +0.0,      3.0,    0.0,   0.0,    0.0,   0.0,   0.0,   1.0,   1.0,    1.0,
        +1.0,      1.0,   -1.0,   1.0,   -1.0,   1.0,   0.0,  -1.0,  -1.0,    0.0,
        -1.0,      1.0,    0.0,  -1.0,    1.0,   1.0,   0.0,   0.0,  -1.0,    0.0,
        +0.0,      0.0,    0.0,   0.0,    0.0,   0.0,   0.0,   0.0,   0.0,    0.0,
        +0.0,      0.0,    0.0,   0.0,    0.0,   0.0
    };
    /** 
     * Coefficients for nutation in obliquity, t part, in 0.1milliarcsec.
     */
    private static final double[] COT = {
        +8.9,  0.5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, -3.1, -0.1,
        -0.6,  0.3,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        +0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        -0.5,  0.0,  0.0, -0.1,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        +0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        +0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        +0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        +0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        +0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        +0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
        +0.0,  0.0,  0.0,  0.0,  0.0,  0.0
    };


    /** 
     * Mean obliquity of the ecliptic.
     */
    private double moe;
    /** 
     * Nutation in longitude current.
     */
    private double dpsiCurrent;
    /** 
     * Nutation in obliquity current.
     */
    private double depsCurrent;
    /** 
     * Nutation in longitude of reference.
     */
    private final double[] dpsiRef;
    /** 
     * Nutation in obliquity of reference.
     */
    private final double[] depsRef;
    /** 
     * Neville interpolation array for dpsi.
     */
    private final double[] dpsiNeville;
    /** 
     * Neville interpolation array for deps.
     */
    private final double[] depsNeville;
    /** 
     * Cached date 
     */
    private AbsoluteDate cachedDate;


    /** 
     * Constructor applying EOP (nutation) corrections
     * 
     * @param date Current date.
     * @param name Name of the frame
     * 
     * @exception OrekitException EOP parameters cannot be read
     */
    protected TEMEFrame(AbsoluteDate date, String name)
        throws OrekitException
    {
        this(true, date, name);
    }
    /** 
     * The applyEOPCorr parameter is available mainly for testing
     * purposes or for consistency with legacy software that don't
     * handle EOP parameters. Beware that setting this parameter to
     * {@code false} leads to very crude accuracy (order of magnitudes
     * are about 1m in LEO and 10m in GEO).
     * 
     * @param applyEOPCorr Apply EOP (nutation) corrections
     * @param date Current date.
     * @param name Name of the frame
     * 
     * @exception OrekitException if EOP parameters are desired but cannot be read
     */
    protected TEMEFrame(boolean applyEOPCorr, AbsoluteDate date, String name)
        throws OrekitException
    {
        super(FramesFactory.getMEME(applyEOPCorr), null , name, 43600.0);
        /*
         * Set up an interpolation model on 12 points with a 1/2 day
         * step this leads to an interpolation error of about 1.7e-10
         * arcseconds
         */
        this.dpsiRef     = new double[N];
        this.depsRef     = new double[N];
        this.dpsiNeville = new double[N];
        this.depsNeville = new double[N];
        /*
         * Synchronize the frame
         */
        this.updateFrame(date);
    }

    /** 
     * Update the frame to the given date with the nutation effects
     * from IERS data.
     * 
     * @param date new value of the date
     * 
     * @exception OrekitException Nutation model data embedded in the
     * library cannot be read
     */
    protected void updateFrame(AbsoluteDate date) throws OrekitException {

        if ((this.cachedDate == null) || !this.cachedDate.equals(date)) {
            Rotation precession;
            {
                /*
                 * Offset from J2000.0 epoch
                 */
                final double tts = date.durationFrom(AbsoluteDate.J2000_EPOCH);
                /*
                 * Evaluate the nutation elements
                 */
                this.setInterpolatedNutationElements(tts);
                /*
                 * Offset from J2000 epoch in julian centuries
                 */
                final double ttc = tts * JULIAN_CENTURY_PER_SECOND;
                /*
                 * Compute the mean obliquity of the ecliptic
                 */
                this.moe = ((MOE_3 * ttc + MOE_2) * ttc + MOE_1) * ttc + MOE_0;
                /*
                 * Get the IAU1980 corrections for the nutation parameters
                 */
                final NutationCorrection nutCorr =
                    ((MEMEFrame) this.getParent()).getNutationCorrection(date);

                final double deps = depsCurrent + nutCorr.getDdeps();
                final double dpsi = dpsiCurrent + nutCorr.getDdpsi();
                /*
                 * Compute the true obliquity of the ecliptic
                 */
                final double toe = moe + deps;
                /*
                 * Elementary rotations for nutation
                 */
                final Rotation r1 = new Rotation(Vector3D.PLUS_I,  toe);
                final Rotation r2 = new Rotation(Vector3D.PLUS_K,  dpsi);
                final Rotation r3 = new Rotation(Vector3D.PLUS_I, -moe);
                /* 
                 * Complete nutation
                 */
                precession = r1.applyTo(r2.applyTo(r3));
            }
            /*
             * Transform from parent MEME
             */
            this.setTransform(new Transform(precession));

            this.cachedDate = date;
        }
    }

    /** 
     * Set the interpolated nutation elements.
     * 
     * @param t offset from J2000.0 epoch in seconds
     */
    protected void setInterpolatedNutationElements(final double t) {

        final int nM12 = (N - 1) / 2;
        if (Double.isNaN(this.tCenter) || (t < this.tCenter) || (t > this.tCenter + this.h)) {
            /*
             * Recompute interpolation array
             */
            this.setReferencePoints(t);
        }
        /*
         * Interpolate nutation elements using Neville's algorithm
         */
        System.arraycopy(this.dpsiRef, 0, this.dpsiNeville, 0, N);
        System.arraycopy(this.depsRef, 0, this.depsNeville, 0, N);
        final double theta = (t - this.tCenter) / this.h;
        final int nM1 = (N - 1);
        for (int j = 1, i; j < N; ++j) {
            for (i = nM1; i >= j; --i) {
                final double c1 = (theta + nM12 - i + j) / j;
                final double c2 = (theta + nM12 - i) / j;
                this.dpsiNeville[i] = ((c1 * this.dpsiNeville[i]) - (c2 * this.dpsiNeville[i - 1]));
                this.depsNeville[i] = ((c1 * this.depsNeville[i]) - (c2 * this.depsNeville[i - 1]));
            }
        }
        this.dpsiCurrent = this.dpsiNeville[nM1];
        this.depsCurrent = this.depsNeville[nM1];
    }
    /** 
     * Set the reference points array.
     * 
     * @param t offset from J2000.0 epoch in seconds
     */
    private void setReferencePoints(final double t) {

        final int nM12 = (N - 1) / 2;
        /*
         * Evaluate new location of center interval
         */
        final double newTCenter = (this.h * Math.floor(t / this.h));
        /*
         * Shift reusable reference points
         */
        int iMin = 0;
        int iMax = N;
        final int shift = (int) Math.rint((newTCenter - this.tCenter) / this.h);
        if (!Double.isNaN(tCenter) && (Math.abs(shift) < N)) {
            if (shift >= 0) {
                int by = (N - shift);
                System.arraycopy(dpsiRef, shift, dpsiRef, 0, by);
                System.arraycopy(depsRef, shift, depsRef, 0, by);
                iMin = by;
            } 
            else {
                int by = (N + shift);
                System.arraycopy(dpsiRef, 0, dpsiRef, -shift, by);
                System.arraycopy(depsRef, 0, depsRef, -shift, by);
                iMax = -shift;
            }
        }
        /*
         * Compute new reference points
         */
        this.tCenter = newTCenter;
        for (int i = iMin; i < iMax; ++i) {
            this.computeNutationElements(newTCenter + (i - nM12) * this.h);
            this.dpsiRef[i] = this.dpsiCurrent;
            this.depsRef[i] = this.depsCurrent;
        }
    }
    /** 
     * Compute nutation elements.  This method applies the IAU-1980
     * theory and hence is rather slow.  It is called by the {@link
     * #setInterpolatedNutationElements(double)} on a small number of
     * reference points only.
     * 
     * @param t offset from J2000.0 epoch in seconds
     */
    protected void computeNutationElements(final double t) {
        /*
         * Offset in julian centuries
         */
        final double tc =  t * JULIAN_CENTURY_PER_SECOND;
        /*
         * Mean anomaly of the Moon
         */
        final double l  = ((F13 * tc + F12) * tc + F110) * tc + F10 + ((F111 * tc) % 1.0) * TWO_PI;
        /*
         * Mean anomaly of the Sun
         */
        final double lp = ((F23 * tc + F22) * tc + F210) * tc + F20 + ((F211 * tc) % 1.0) * TWO_PI;
        /*
         * L - &Omega; where L is the mean longitude of the Moon
         */
        final double f  = ((F33 * tc + F32) * tc + F310) * tc + F30 + ((F311 * tc) % 1.0) * TWO_PI;
        /*
         * Mean elongation of the Moon from the Sun
         */
        final double d  = ((F43 * tc + F42) * tc + F410) * tc + F40 + ((F411 * tc) % 1.0) * TWO_PI;
        /*
         * Mean longitude of the ascending node of the Moon
         */
        final double om = ((F53 * tc + F52) * tc + F510) * tc + F50 + ((F511 * tc) % 1.0) * TWO_PI;

        /*
         * Loop size
         */
        final int n = CL.length;
        /*
         * Initialize nutation elements.
         */
        double dpsi = 0.0;
        double deps = 0.0;
        /*
         * Sum the nutation terms from smallest to biggest.
         */
        for (int j = (n - 1); j >= 0; j--)
        {
            /*
             * Set up current argument.
             */
            final double arg = CL[j] * l + CLP[j] * lp + CF[j] * f + CD[j] * d + COM[j] * om;
            /*
             * Accumulate current nutation term.
             */
            final double s = SL[j] + SLT[j] * tc;
            final double c = CO[j] + COT[j] * tc;
            if (s != 0.0) 
                dpsi += (s * Math.sin(arg));
            if (c != 0.0) 
                deps += (c * Math.cos(arg));
        }
        /*
         * Convert results from 0.1 mas units to radians.
         */
        final double rr = (RADIANS_PER_ARC_SECOND * 1.e-4);
        this.dpsiCurrent = (dpsi * rr);
        this.depsCurrent = (deps * rr);
    }
}
